

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/gicp.h>

#include <geometry_utils/Transform3.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <string>
#include "time_counter.hpp"
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <point_new.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
using namespace std;


ros::Publisher pub_cloud2;
ros::Publisher pub_image;

template <typename Point>
void pubpc(ros::Publisher &pub, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    pub.publish(msgs);
}

int i = 0;
const int jump = 1;
void velodyne_callback(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    if(cloud->header.stamp.sec<1600000000){
        pub_cloud2.publish(*cloud);
    }

}

void ImageCallback(const sensor_msgs::Image::ConstPtr &img)
{
    if(img->header.stamp.sec<1000000000){
        pub_image.publish(*img);
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "bag2pcd");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/pointcloud_lidar3", 10, velodyne_callback);
    ros::Subscriber sub_img = nh.subscribe<sensor_msgs::Image>("/camera79", 1, &ImageCallback);
    pub_cloud2 = nh.advertise<sensor_msgs::PointCloud2>("/pointcloud_lidar31", 1);
    pub_image = nh.advertise<sensor_msgs::Image>("/camera791", 1);

    ros::spin();

    return 0;
}
